package robocup.component.worldmodel;

import robocup.component.SConf;
import robocup.component.WorldModel;
import robocup.component.ControllerNumbers.PLAY_MODE;
import robocup.component.geometry.Vektor;
import robocup.component.infotypes.InfoReceiver;
import robocup.component.infotypes.PlayModeInfo;
import robocup.component.infotypes.PlayerSayInfo;
import robocup.component.infotypes.PlayerTypesInfo;
import robocup.component.infotypes.RefereeInfo;
import robocup.component.infotypes.SenseBodyInfo;
import robocup.component.infotypes.VisualInfo;
import robocup.component.worldobjects.Ball;
import robocup.component.worldobjects.Player;
import robocup.component.worldobjects.Player.UPDATE_MODE;

/**
 * The {@link CollisionModel} provides method for correcting previously done
 * updates in the {@link MeModel} and {@link BallModel} when a collision with
 * the Ball occured. If two objects collide the speed of each objects is
 * multiplied by -0.1 and they are set outside each other so that you will never
 * find 2 objects over- laping (at least in PLAY_MODE==PLAY_ON). Most of the
 * times it is easy to tell wether we collided as our speed differs
 * significantly from the expected speed. However, there remain 2 cases where a
 * collision with the ball is hard to detect:
 * 
 * 1. Another player is near and it is not clear wether we collided with the
 * ball or with this player. Using the information contained in a visual info
 * might help in this case if the ball speed is explicitly seen or the ball
 * and/or other player is too far for a collision.
 * 
 * 2. If we are very slow it is hard to tell wether our speed was multiplied by
 * -0.1 or if this effect was caused by noise. Again, using the information
 * contained in the visual info might help here.
 * 
 */
public class CollisionModel
    implements InfoReceiver {

  private final WorldModel world;

  // true if we collided with something
  private boolean    collided                    = true;

  // true if that something was the ball
  private boolean    collidedWithBall            = false;

  // true if another player might be the object we collided with
  private boolean    mightHaveCollidedWithPlayer = false;

  // the posiiton we were supposed to be
  private final Vektor     expectedPosition            = Vektor.getDummyInstance();

  /**
   * Constructor - a model to detect collisions with the ball.
   * 
   * @param world -
   *          the current world-model
   */
  public CollisionModel(
      final WorldModel world) {

    this.world = world;
  }

  /**
   * resets all cycle data.
   * 
   */
  private void reset() {

    this.collided = false;
    this.collidedWithBall = false;
    this.mightHaveCollidedWithPlayer = false;
  }

  /**
   * updating model after BS.
   * 
   * @param info
   */
  public void update(final SenseBodyInfo info) {

    this.reset();

    // only play_on
    if (this.world.getPlayMode() != PLAY_MODE.PLAY_ON) {

      // if we collided with the ball in an dead cycle, the server doesn't
      // move the ball outside the player here the ball is set outside the
      // player preventing strange method results
      if (this.world.getSelfRef().getDistance(this.world.getBallRef()) < this.world
          .getSelfRef().getPConf().PLAYER_SIZE + SConf.getInstance().BALL_SIZE) {
        final Ball ballRef = this.world.getBallRef();
        final double angle = this.world.getSelfRef()
            .getAngleTo(ballRef.getPosition());

        this.world
            .getSelfRef()
            .getPosition()
            .pointAtPolar(
                this.world.getMyPConf().PLAYER_SIZE + SConf.getInstance().BALL_SIZE + 0.001,
                angle);
        this.world.getSelfRef().getPosition().subFromThis(
            this.world.getBallRef().getPosition());
        this.world.getSelfRef().getPosition().multThis(-1);

        this.world.getExpectedPlayers()[this.world.getNumber() - 1]
            .copy(this.world.getSelfRef());
        this.world.getNextPlayers()[this.world.getNumber() - 1].copy(this.world
            .getSelfRef());
      }
      return;
    }

    // as we collided the speed given in the BS is not the speed used by the
    // server for calculating our movement. for checking for collision we have
    // to undo this effect
    this.expectedPosition.copy(this.world.getSelfRef().getPosition());

    // collided
    if (this.checkIfCollided()) {
      this.collided = true;

      // calculate position as it would have been without collision
      this.expectedPosition.subFromThis(this.world.getSelfRef().getSpeed()
          .mult(11 / this.world.getSelfRef().getPConf().PLAYER_DECAY));
      this.world.getSelfRef().setPosition(this.expectedPosition);

      this.world.getExpectedPlayers()[this.world.getNumber() - 1]
          .copy(this.world.getSelfRef());
      this.world.getNextPlayers()[this.world.getNumber() - 1].copy(this.world
          .getSelfRef());

      // test if it is possible that we collided with someone (used here
      // for setting security margins)
      this.mightHaveCollidedWithPlayer = this
          .checkIfMightHaveCollidedWithPlayer();
    }

    // collided with ball
    if (this.checkIfCollidedWithBallAfterBS()) {
      this.collided = true;
      this.collidedWithBall = true;

      // collision with ball has nearly no effect on the players last
      // movement
      this.world.getSelfRef().setPosition(this.expectedPosition);

      this.world.getExpectedPlayers()[this.world.getNumber() - 1]
          .copy(this.world.getSelfRef());
      this.world.getNextPlayers()[this.world.getNumber() - 1].copy(this.world
          .getSelfRef());

      // the ball can't be inside the player, instead the server moves it
      // outside
      this.moveCollidedBallToPlayerMargin();

      // "stop" ball
      this.world.getBallRef().getSpeed().multThis(-0.1);
      this.world.getBallRef().setSpeedReliable(false);
    }
    else {
      this.collidedWithBall = false;
    }

    // if the ball was predicted inside the player remove this effect
    // (as some methods go mad if the ball is inside)
    if (this.world.getSelfRef().getDistance(this.world.getBallRef()) < this.world
        .getSelfRef().getPConf().PLAYER_SIZE + SConf.getInstance().BALL_SIZE) {
      this.moveCollidedBallToPlayerMargin();
    }

  }

  /**
   * updating model after VI.
   * 
   * @param info
   */
  public void update(final VisualInfo info) {

    // collided with ball
    // if the speed is not seen the ballSpeed is calculated using current
    // and last known ball position (ballModel). if we collided with the ball
    // this has to be corrected
    if (this.checkIfCollidedWithBallAfterVI()) {
      this.world.getBallRef().getSpeed().multThis(-0.1);
      this.world.getBallRef().setSpeedReliable(false);
      this.collided = true;
      this.collidedWithBall = true;
      this.world.resetFilterAfterCollisionWithBall();
    }
    else {
      this.collidedWithBall = false;
    }

    // if the ball was seen inside the player due to noise remove this
    // effect (as some methods go mad if the ball is inside)
    if (this.world.getSelfRef().getDistance(this.world.getBallRef()) < this.world
        .getSelfRef().getPConf().PLAYER_SIZE + SConf.getInstance().BALL_SIZE) {
      this.moveCollidedBallToPlayerMargin();
    }

    // if we didn't collide with the ball we collided with someone
    // find out who it was and set his speed to 0.
    if (!this.collidedWithBall && this.mightHaveCollidedWithPlayer) {
      final int collidedPlayer = this.checkIfCollidedWithPlayerAfterVI();

      if (collidedPlayer >= 0) {
        final Player p = this.world.getAllPlayers()[collidedPlayer];
        p.getSpeed().reset();
      }
    }
  }

  /**
   * true if a reliable player was near enough that a collision with this player
   * was possible.
   */
  private boolean checkIfMightHaveCollidedWithPlayer() {

    for (final Player p : this.world.getAllPlayers()) {
      if (!p.isMe() && p.isReliable() && this.world.getAge(p) == 1 && p
          .getDistance(this.expectedPosition) < p.getPConf().PLAYER_SIZE + this.world
          .getSelfRef().getPConf().PLAYER_SIZE + p.getPConf().PLAYER_SPEED_MAX) {
        return true;
      }
    }
    return false;
  }

  /**
   * the ball is moved in radial direction outside the player.
   */
  private void moveCollidedBallToPlayerMargin() {

    final Ball ballRef = this.world.getBallRef();
    final double angle = this.world.getSelfRef().getAngleTo(ballRef.getPosition());

    ballRef.getPosition().pointAtPolar(
            this.world.getMyPConf().PLAYER_SIZE + SConf.getInstance().BALL_SIZE 
            + 0.001, angle);
    ballRef.getPosition().addToThis(this.world.getSelfRef().getPosition());
  }

  /**
   * the method checks if a collision has occured. A collision has (probably)
   * occured if our Speed direction is in the opposite as the predicted
   * direction. If we are very slow noise might lead to wrond results.
   * 
   * @return true if collision seems probable
   */
  private boolean checkIfCollided() {

    final Vektor speedMe = this.world.getSelfRef().getSpeed();
    // the expected self (set in World before an BS-Update)
    final Player predMe = this.world.getExpectedSelf();
    Vektor predSpeedMe;
    if (predMe != null) {
      predSpeedMe = predMe.getSpeed();
    }
    else {
      System.out
          .println(this.world.getCycle() + " GOT NO ME PREDICTION! (MeModel)");
      // no speed was predicted -> not enough infos for deciding
      return false;
    }
    // the speed that would result after a collision
    final Vektor speedAfterCollision = predSpeedMe.cloned();
    speedAfterCollision.multThis(-0.1);

    // the players speed is like the speed he would have after a collision
    final boolean speedChanged = speedMe.getDistance(speedAfterCollision) < 0.03;

    // the speed direction has changed (server mult. speed by -.1) when
    // collision
    final boolean speedDirChanged = ((Math.signum(predSpeedMe.x) != Math
        .signum(speedMe.x) || speedMe.x < 0.01) && (Math.signum(predSpeedMe.y) != Math
        .signum(speedMe.y) || speedMe.y < 0.01));

    // it is not noise that changed the speed
    final boolean speedRelevant = (predSpeedMe.getLength() > 0.03);

    // the return value
    final boolean collided = (speedChanged && speedDirChanged && speedRelevant);

    return collided;
  }

  /**
   * This method checks if the ball was the cause of the collision. It's called
   * only after BS-Infos.
   * 
   */
  public boolean checkIfCollidedWithBallAfterBS() {

    // only play_on
    // the ball can be inside a player in dead/standard situations (i.e.
    // kick off)
    if (this.world.getPlayMode() != PLAY_MODE.PLAY_ON || this.world.isViBased()) {
      return false;
    }

    final Ball ballRef = this.world.getBallRef();
    final double secureDist = (this.mightHaveCollidedWithPlayer) ? 0.1 : 0.5;

    boolean ballNear = (this.expectedPosition
        .getDistance(ballRef.getPosition()) < SConf.getInstance().BALL_SIZE + this.world
        .getMyPConf().PLAYER_SIZE + secureDist);

    // ball far -> no collsion
    if (!ballNear) {
      return false;
    }

    // ball near && collided -> collided with ball
    // (yes. we can't be sure here, but some risk always remains ;-))
    if (this.collided) {
      return true;
    }
    else {
      // if we are too slow the detection of a collision only by the
      // players
      // speed isn't safe. so if we are slow and the ball is inside us
      // assume
      // we collided with the ball.
      final boolean ballInside = this.expectedPosition.getDistance(ballRef
          .getPosition()) < SConf.getInstance().BALL_SIZE + this.world
          .getMyPConf().PLAYER_SIZE;
      final boolean meToSlowToDetectCollision = this.world.getSelfRef().getSpeed()
          .getLength() < 0.02;

      if (ballInside && meToSlowToDetectCollision) {
        return true;
      }
    }
    return false;
  }

  /**
   * After a VI the ball speed is (if not seen) calculated using last and
   * current position (ballModel). If we collided we have to correct this speed.
   * We assume that we collided with the ball if the ball is near and we
   * collided with something.
   * 
   * @return true if collided with ball
   */
  private boolean checkIfCollidedWithBallAfterVI() {

    final Ball ballRef = this.world.getBallRef();
    final double secureDistance = (this.collided) ? ((this.mightHaveCollidedWithPlayer) ? 0.1 : 0.15) : 0.05;

    if ((this.collided || this.world.getSelfRef().getSpeed().getLength() < 0.02) 
        && this.world.getAge(ballRef) == 0 && !ballRef.speedIsReliable()) {
      if (this.world.getSelfRef().getDistance(ballRef) < this.world
          .getSelfRef().getPConf().PLAYER_SIZE + SConf.getInstance().BALL_SIZE + secureDistance) {
        return true;
      }
    }
    return false;
  }

  /**
   * checks if we collided with an other player. this method returns true if we
   * collided and someone is felt very near.
   * 
   * @return
   */
  private int checkIfCollidedWithPlayerAfterVI() {

    if (!this.collided) {
      return -1;
    }

    final double myRad = this.world.getSelfRef().getPConf().PLAYER_SIZE;

    for (int i = 0; i < this.world.getAllPlayers().length; i++) {
      final Player p = this.world.getAllPlayers()[i];

      if (this.world.getNumber() == i + 1 || !p.isReliable() || p.getAge(
          this.world.getBodyCycle(), UPDATE_MODE.SEEN_NOTEAM) != 0 || p
          .speedIsReliable()) {
        continue;
      }

      if (this.world.getSelfRef().getDistance(p) < myRad + p.getPConf().PLAYER_SIZE + 0.1) {
        return i;
      }
    }

    return -2;
  }

  /*
   * (non-Javadoc)
   * 
   * @see robocup.component.worldmodel.IModel#update(robocup.component.infotypes.RefereeInfo)
   */
  public void update(final RefereeInfo info) {

  }

  /*
   * (non-Javadoc)
   * 
   * @see robocup.component.worldmodel.IModel#update(robocup.component.infotypes.PlayModeInfo)
   */
  public void update(final PlayModeInfo info) {

  }

  /*
   * (non-Javadoc)
   * 
   * @see robocup.component.worldmodel.IModel#update(robocup.component.infotypes.PlayerSayInfo)
   */
  public void update(final PlayerSayInfo info) {

  }

  /**
   * @return true if collided
   */
  public boolean collided() {

    return this.collided;
  }

  /**
   * @return Returns the collidedWithBall.
   */
  public boolean collidedWithBall() {

    return this.collidedWithBall;
  }

  /* (non-Javadoc)
   * @see robocup.component.infotypes.InfoReceiver#update(robocup.component.infotypes.PlayerTypesInfo)
   */
  public void update(final PlayerTypesInfo info) {

    // TODO Auto-generated method stub
    
  }

}
